from FingerPrint_interface.fingerprint_interface import Fingerprint, FingerprintDatabase
from FingerPrint_interface.temporal_fingerprint import TemporalFingerprintSeq, TemporalFingerprint
from CV_interface.temporal_cv import TemporalCVSeq, TemporalCV
from ParticleFilter.particleFilterMain import particleFilterMain
from PDR.input import dataInputPDR, PDR_draw_trace, PDR_draw_trace_cv, acc_draw
from PDR.mainActivity import mainActicity
import matplotlib.pyplot as plt
import math

"""
1:干扰物多
2:时间延迟大
3:同时有干扰与延迟
4:二维平面中的直线运动

"""


test_data_dir = 'test_data/2'
place = '11-211'
# place = 'cooridor'
path = test_data_dir + '/pdr'

acc_filename = path + '/acc.txt'
gyr_filename = path + '/gyr.txt'
mag_filename = path + '/mag.txt'
acc_data = dataInputPDR(acc_filename)
gyr_data = dataInputPDR(gyr_filename)
mag_data = dataInputPDR(mag_filename)

acc_draw(acc_data)
acticity = mainActicity()
result = acticity.dataProcess(acc_data, gyr_data, mag_data)

trajectory = []
stepYaw = []
x = 0
y = 0
lsc = 0
for (timestamp, stepCount, StrideLength, direction) in result:
    lastTimestamp = timestamp
    if direction == direction:
        if stepCount > lsc and (timestamp < 1531558526527 or timestamp > 1531558529849):
            x += math.cos(math.radians(direction)) * StrideLength * (stepCount - lsc)
            y += math.sin(math.radians(direction)) * StrideLength * (stepCount - lsc)
            print(timestamp, direction, StrideLength, stepCount - lsc, x, y)
            trajectory.append((x, y))
            if place == 'cooridor':
                if direction < -90.0:
                    direction = -180.0
                else:
                    direction = 0
            stepYaw.append((timestamp + 2000, 90.0 + direction, StrideLength * (stepCount - lsc)))
        lsc = stepCount
    else:
        print(direction)
#print(trajectory)
PDR_draw_trace(trajectory)

if place == '11-211':
    fp_database = FingerprintDatabase('fingerprint_data/11-211')
    tem_fp_seq = TemporalFingerprintSeq(test_data_dir, fp_database)
    #print(str(tem_fp_seq))
    tem_cv_seq = TemporalCVSeq(test_data_dir)
    PDR_draw_trace_cv(trajectory, tem_cv_seq.ground_truth_seq(stepYaw[0][0]))
    #print(str(tem_cv_seq))
    #tem_cv_seq.plot_out()
    print('data load ok')
    deviation = particleFilterMain(tem_cv_seq, tem_fp_seq, stepYaw, fp_database, place='11-211')

if place == 'kejiyuan':
    fp_database = FingerprintDatabase('fingerprint_data/kejiyuan')
    tem_fp_seq = TemporalFingerprintSeq(test_data_dir, fp_database)
    # print(str(tem_fp_seq))
    tem_cv_seq = TemporalCVSeq(test_data_dir)
    # print(str(tem_cv_seq))
    # tem_cv_seq.plot_out()
    deviation = particleFilterMain(tem_cv_seq, tem_fp_seq, stepYaw, fp_database, place='kejiyuan')

if place == 'cooridor':
    fp_database = FingerprintDatabase('fingerprint_data/cooridor')
    tem_fp_seq = TemporalFingerprintSeq(test_data_dir, fp_database)
    # print(str(tem_fp_seq))
    tem_cv_seq = TemporalCVSeq(test_data_dir)
    # print(str(tem_cv_seq))
    # tem_cv_seq.plot_out()
    deviation = particleFilterMain(tem_cv_seq, tem_fp_seq, stepYaw, fp_database, place='cooridor')

if place == 'office':
    fp_database = FingerprintDatabase('fingerprint_data/office')
    tem_fp_seq = TemporalFingerprintSeq(test_data_dir, fp_database)
    tem_cv_seq = TemporalCVSeq(test_data_dir)
    deviation = particleFilterMain(tem_cv_seq, tem_fp_seq, stepYaw, fp_database, place='office')

if place == 'new211':
    fp_database = FingerprintDatabase('fingerprint_data/new211')
    tem_fp_seq = TemporalFingerprintSeq(test_data_dir, fp_database)
    tem_cv_seq = TemporalCVSeq(test_data_dir)
    deviation = particleFilterMain(tem_cv_seq, tem_fp_seq, stepYaw, fp_database, place='new211')

print(deviation)
deviation = sorted(deviation)
c = len(deviation)
index = [float(i + 1) / c for i in range(c)]
# plt.clf()
# plt.plot(deviation, index)
# plt.savefig('tmp\\cdf.png')
#print(deviation)
print(sum(deviation) / len(deviation))

# out_file = open('deviation.txt', 'w')
# for x in deviation:
#     out_file.write('{}\n'.format(x))
# out_file.close()
#








